#pragma once

#include <iostream>
#include <graphics.h>
#include <cmath>
#include <ctime>
#include <vector>
#include <string>

constexpr double SWIDTH = 1200.0; // 窗口宽度
constexpr double SHEIGHT = 1200.0; // 窗口高度
constexpr double PI = 3.1415926535897; // 圆周率
constexpr bool SHOWCIRCLE = false; // 是否绘制轨迹 
constexpr double DELAYTIME = 20.0; // 间隔时间， ms
constexpr double CHANGETIME = 1000; // 换挡时间， ms

// 点
class Point {
  public:
    // 默认构造函数
    Point() = default;
    // 带参自定义构造函数
    Point(const double& p_x, const double& p_y, const double& p_theta = 0.0, const double& p_R = 0.0); 
    void ShowPoint(); // 绘制点
    void PointMove(const double& speed_x, const double& speed_y); // 点的移动
    void PointTurn(const Point& center, const double& turn_speed); // 点绕center旋转，turn_speed为角速度
    double DistanceTo(const Point& p) const; //两点间距离
    double ThetaTo(const Point& p) const; // this->相对于p点的角度
  public:
    double x; // 横坐标
    double y; // 纵坐标
    double thetaP; // 角度
    double Rp; // 旋转半径
    int r = 5; // 绘制半径
};
/*******************************向量*********************************/
class Vec2d {
  public:
    Vec2d() = default;
    Vec2d(const double& new_x, const double& new_y, const bool& flag); // 通过两个值进行构造向量，flag仅用于区分
    Vec2d(const Point& p_start, const Point& p_end); // 通过两个点构造向量
    Vec2d(const double& length, const double& angle); // 通过长度和方向角构造向量
    double Length(); // 向量的模
    double CrossProd(const Vec2d& other) const; // 向量叉乘
    double InnerProd(const Vec2d& other) const; // 向量点乘  
  public:
    double x;
    double y;
    // bool flag = false;

};

/*********************************全局函数***************************/
// 时间延时
void Delay(const double& time);
// 将角度修正到[-pi, pi]之间
double NormalizeAngle(const double& theta);
// 角度误差修正
void CorrectAngleError(double& target_angle, const double& delta_theta);
// 点到直线的距离
double DisPointToLine(const Point& p, const Point& p_start, const Point& p_end);


